#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Path.h>
#include <sstream>
#include <map>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <iostream>
#include <fstream>
#include <cassert>
#include <string>

using boost::property_tree::ptree;
using boost::property_tree::read_json;
using boost::property_tree::write_json;

using namespace std;

vector<string> pathPlan(vector< vector<string> > inputMap, vector< vector<int> > inputObstacle, string startPoint, string targetPoint);
vector<geometry_msgs::PoseStamped> genPathDisplay(vector<string> plan_result, map< string, vector<double> > cmapData);
std::string int2str(int i);

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher path_pub = n.advertise<nav_msgs::Path>("path_display", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);

    int x_length = 10;
    int y_length = 10;

    // Generate sematic grid map
    vector< vector<string> > inputMap(x_length);
    for (int i = 0; i < x_length; i++)
    {
        for (int j = 0; j < y_length; j++)
        {
            inputMap[i].push_back("A" + int2str(i) + "_" + int2str(j) );
        }
    }

    // Generate obstacle map
    vector< vector<int> > inputObstacle(x_length);
    for (int i = 0; i < x_length; i++)
    {
        for (int j = 0; j < y_length; j++)
        {
            inputObstacle[i].push_back(0);
        }
    }
    // read obstacle information file
    ifstream in("src/exercise_1/scripts/obsData.txt");
    string filename;
    string line;

    std::vector<string> obsData;
    if (in)
    {
        while (getline (in, line))
        {
            obsData.push_back(line);
            //cout << line << endl;
        }
    }
    else
    {
        cout << "No such obstacle information file" << endl;
    }
    for (int i = 0; i < obsData.size(); i++)
    {
        int num = atoi( obsData[i].c_str() );
        int x_index =  num / x_length;
        int y_index = num % x_length;
        inputObstacle[x_index][y_index] = 1;
    }

    map< string, vector<double> > cmapData;
    double mapResolution = 0.5;
    for (int i = 0; i < x_length; i++)
    {
        for (int j = 0; j < y_length; j++)
        {
            vector<double> temp;
            temp.clear();
            temp.push_back(i * mapResolution);
            temp.push_back(j * mapResolution);
            cmapData.insert(make_pair(inputMap[i][j], temp));
        }
    }


    vector<string> plan_result = pathPlan(inputMap, inputObstacle, "A0_0", "A9_9");
    vector<geometry_msgs::PoseStamped> poses_display = genPathDisplay(plan_result, cmapData);

    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "map";
    odom_trans.child_frame_id = "base_link";

    nav_msgs::Path path_display;
    path_display.header.stamp = ros::Time::now();
    path_display.header.frame_id = "map";
    path_display.poses = poses_display;

    int i = 0;

    while (ros::ok()) {

        // update transform
        // (moving in a circle with radius)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = poses_display[i].pose.position.x;
        odom_trans.transform.translation.y = poses_display[i].pose.position.y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = poses_display[i].pose.orientation;

        //send the joint state and transform
        path_pub.publish(path_display);
        broadcaster.sendTransform(odom_trans);


        if (i < poses_display.size() - 1)
        {
            i += 1;
        }
        else
        {
            i += 0;
        }

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }

    return 0;
}

vector<geometry_msgs::PoseStamped> genPathDisplay(vector<string> plan_result, map< string, vector<double> > cmapData)
{
    vector<geometry_msgs::PoseStamped> poses_display;
    poses_display.clear();

    int num_points = plan_result.size();
    if (num_points > 1)
    {
        for (int i = 0; i < num_points - 1; i++)
        {
            string start_name = plan_result[i];
            string end__name = plan_result[i + 1];

            map< string, vector<double> >::iterator s_it;
            s_it = cmapData.find(start_name);
            vector<double> start_posi = s_it->second;

            map< string, vector<double> >::iterator e_it;
            e_it = cmapData.find(end__name);
            vector<double> end_posi = e_it->second;

            ROS_INFO_STREAM("start x: " << start_posi[0]);
            ROS_INFO_STREAM("start y: " << start_posi[1]);
            ROS_INFO_STREAM("end x: " << end_posi[0]);
            ROS_INFO_STREAM("end y: " << end_posi[1]);

            double deltax = end_posi[0] - start_posi[0];
            double deltay = end_posi[1] - start_posi[1];

            double angle;
            if (deltax == 0 && deltay != 0)
            {
                angle = 1.57;
            }
            else if (deltax == 0 && deltay == 0)
            {
                angle = 0;
            }
            else
            {
                angle = atan( deltay / deltax );
            }

            ROS_INFO_STREAM("Orientation angle: " << angle);

            int viapoint_size = sqrt(deltax*deltax+deltay*deltay) / 0.1;
            if (viapoint_size == 0)
                viapoint_size = 1;

            for (int i = 0; i < (viapoint_size+1); i++)
            {
                geometry_msgs::PoseStamped temp;
                temp.header.stamp = ros::Time::now();
                temp.header.frame_id = "map";
                temp.pose.position.x = start_posi[0] + i * deltax / viapoint_size;
                temp.pose.position.y =  start_posi[1] + i * deltay / viapoint_size;
                temp.pose.position.z = 0;
                temp.pose.orientation = tf::createQuaternionMsgFromYaw(angle + 1.57);
                poses_display.push_back(temp);
            }

        }
    }
    else
    {
        string start_name = plan_result[0];
        map< string, vector<double> >::iterator s_it;
        s_it = cmapData.find(start_name);
        vector<double> start_posi = s_it->second;

        geometry_msgs::PoseStamped temp;
        temp.header.stamp = ros::Time::now();
        temp.header.frame_id = "map";
        temp.pose.position.x = start_posi[0];
        temp.pose.position.y =  start_posi[1];
        ROS_INFO_STREAM("start x: " << start_posi[0]);
        ROS_INFO_STREAM("start y: " << start_posi[1]);
        temp.pose.position.z = 0;
        temp.pose.orientation = tf::createQuaternionMsgFromYaw( 1.57);
        poses_display.push_back(temp);

    }
    return poses_display;

}

vector<string> pathPlan(vector< vector<string> > inputMap, vector< vector<int> > inputObstacle,
                        string startPoint, string targetPoint)
{
    vector<string> plan_result;
    plan_result.push_back(startPoint);
    plan_result.push_back(targetPoint);
    return plan_result;

}

std::string int2str(int i) {

    std::string s;
    std::stringstream ss(s);
    ss << i;
    return ss.str();
}
